/*
* @Description: Matrix竞赛机器人PID控制常用技巧

* @Author: JC_Zhu
* @Email: zjujiangchaozhu@gmail.com
* @Date:   2022-05-19 09:25:51
* @Modified by: Yinshen Wang
* @Email:  wangyinshen@zju.edu.cn
* @Modified time: 2022-06-15 23:59:30
*/

#include "PID_Differential_Control.h"

PID_Differential_Control::PID_Differential_Control(Matrix_ESP32* matrix, MPU6050* mpu6050,\
                          ESP32Encoder* left_encoder, ESP32Encoder *right_encoder, \
                          int left_motor_id, int right_motor_id) :\
                          matrix(matrix), left_encoder(left_encoder), right_encoder(right_encoder),\
                          mpu6050(mpu6050), left_motor_id(left_motor_id), right_motor_id(right_motor_id) {
}

PID_Differential_Control::~PID_Differential_Control(){

}


void PID_Differential_Control::inverse_kinematics() {
    /* 
    ||      ||
    || 0  1 ||
    ||      || 
    */
    motor_w_desire[0] = (exec_vel - 0.5 * WIDTH_OF_CAR * exec_wc)*INV_WHEEL_RADIUS;
    motor_w_desire[1]=  (exec_vel + 0.5 * WIDTH_OF_CAR * exec_wc)*INV_WHEEL_RADIUS;
    motor_pwm_desire[0] = motor_w_desire[0]*W2PWM_FACTOR;
    motor_pwm_desire[1] = motor_w_desire[1]*W2PWM_FACTOR;  
}

void PID_Differential_Control::task_init(long cur_time) {
    task_ts = cur_time;
    current_update_t = cur_time;
    last_update_t = cur_time;
    exec_vel = 0;
    exec_wc = 0;

    if (task_type == GO_FORWARD || task_type == GO_FORWARD_V2) {
        dir = (pos_x >= 0) ? 1 : -1;
        pos_x = fabs(pos_x);
        max_vel2 = max_vel*max_vel;
        str_vel2 = str_vel*str_vel;
        end_vel2 = end_vel*end_vel;
        ref_pos = (2*max_vel2 - str_vel2 - end_vel2)/(2*max_acc);

        if (pos_x < ref_pos) {
            t1 = (sqrt(max_acc*pos_x + 0.5*(str_vel2+end_vel2)) - str_vel) / max_acc;
            t2 = t1;
            t3 = (sqrt(4*max_acc*pos_x + (str_vel2 + end_vel2)) - (str_vel + end_vel)) / max_acc;
            bangbang_type = 0;
        } else {
            t1 = (max_vel - str_vel) / max_acc;
            t2 = (2*max_acc*pos_x + str_vel2 + end_vel2 - 2*max_vel*str_vel) / (2*max_acc*max_vel);
            t3 = (2*max_acc*pos_x + (max_vel - str_vel)*(max_vel - str_vel) + (max_vel - end_vel)*(max_vel - end_vel)) / (2*max_acc*max_vel);
            bangbang_type = 1;
        }
    } else if  (task_type == ROTATION) {
        dir = (angle_degree >= 0) ? 1 : -1;
        angle_degree = abs(angle_degree);
        angle_rad = angle_degree*0.017453292;
        if (angle_rad < max_wc*max_wc/max_angular_acc) {
            t1 = sqrt(angle_rad/max_angular_acc);
            t2 = t1;
            t3 = 2*t1;
            bangbang_type = 0;
        } else {
            t1 = max_wc/max_angular_acc;
            t2 = angle_rad/max_wc;
            t3 = angle_rad/max_wc + max_wc/max_angular_acc;
            bangbang_type = 1;
        }
    } else if (task_type == REVOLUTION || task_type == REVOLUTION_V2) {
        dir = (angle_degree >= 0) ? 1 : -1;
        angle_degree = abs(angle_degree);
        angle_rad = angle_degree*0.017453292;
        pos_x = angle_rad*radius;
        max_wc = max_vel/radius;

        max_vel2 = max_vel*max_vel;
        str_vel2 = str_vel*str_vel;
        end_vel2 = end_vel*end_vel;
        ref_pos = (2*max_vel2 - str_vel2 - end_vel2)/(2*max_acc);
        if (pos_x < ref_pos) {
            t1 = (sqrt(max_acc*pos_x + 0.5*(str_vel2+end_vel2)) - str_vel) / max_acc;
            t2 = t1;
            t3 = (sqrt(4*max_acc*pos_x + (str_vel2 + end_vel2)) - (str_vel + end_vel)) / max_acc;
            bangbang_type = 0;
        } else {
            t1 = (max_vel - str_vel) / max_acc;
            t2 = (2*max_acc*pos_x + str_vel2 + end_vel2 - 2*max_vel*str_vel) / (2*max_acc*max_vel);
            t3 = (2*max_acc*pos_x + (max_vel - str_vel)*(max_vel - str_vel) + (max_vel - end_vel)*(max_vel - end_vel)) / (2*max_acc*max_vel);
            bangbang_type = 1;
        }
    }

    mpu6050->update();
    angleZ_taskstart_degree = mpu6050->getAngleZ();
    angleZ_taskstart_rad = angleZ_taskstart_degree*0.017453292;
    left_encoder_start = left_encoder->getCount();
    last_encoder[0] = left_encoder_start;
    right_encoder_start = right_encoder->getCount();
    last_encoder[1] = right_encoder_start;
}

void PID_Differential_Control::incremental_pid_motor_id(int motor_id) {
    int err = motor_pwm_desire[motor_id] - motor_pwm_measure[motor_id];
    pid_err_sum[motor_id] += err;
    motor_pwm_out[motor_id] += inc_Kp[motor_id]*(err-last_pid_err[motor_id])+\
                               inc_Ki[motor_id]*err;

    if(motor_pwm_out[motor_id] < -255)  motor_pwm_out[motor_id] = -255;
    if(motor_pwm_out[motor_id] >  255)  motor_pwm_out[motor_id] =  255;

    last_pid_err[motor_id] = err;
  }

void PID_Differential_Control::angle_loop_pid() {
    angleZ_desire_rad = get_desire_angle(millis());
    angleZ_desire_degree = angleZ_desire_rad*57.29578;
    angleZ_measure_degree = angleZ_current_degree-angleZ_taskstart_degree;
    angleZ_measure_rad = angleZ_measure_degree*0.0174;

    float angleZ_err_rad = angleZ_desire_rad - angleZ_measure_rad;
    angleZ_err_sum_rad += angleZ_err_rad;
    float angle_loop_pid_out =  Kp_angleZ*angleZ_err_rad + \
                                Ki_angleZ*angleZ_err_sum_rad + \
                                Kd_angleZ*(angleZ_err_rad - angleZ_last_err_rad);
    angleZ_last_err_rad = angleZ_err_rad;
    exec_wc = angle_loop_pid_out;
}

void PID_Differential_Control::position_loop_pid() {
    posX_desire = get_desire_pos(millis());
    posX_measure = ((left_encoder_current - left_encoder_start) + \
                    (right_encoder_current - right_encoder_start)) * 2.7150e-6;
    float posX_err = posX_desire - posX_measure;
    posX_err_sum += posX_err;
    float pos_loop_pid_out =  Kp_posX*posX_err + \
                              Ki_posX*posX_err_sum + \
                              Kd_posX*(posX_err - posX_last_err);
    posX_last_err = posX_err;
    exec_vel = pos_loop_pid_out;
}

float PID_Differential_Control::get_desire_angle(long cur_time) {
    float delta_t = 0.001 * (cur_time - task_ts);
    float res = 0;

    if (task_type == ROTATION) {
        if(bangbang_type == 0) {
            if (delta_t <= t1) {
                res =  0.5*max_angular_acc*delta_t*delta_t;
            } else if (delta_t > t1 && delta_t <= t3) {
                res =  angle_rad - 0.5*max_angular_acc*(delta_t-t3)*(delta_t-t3);
            } else {
                res = angle_rad;
            }
        }

        if (bangbang_type == 1) {
            if (delta_t <= t1) {
                res = 0.5*max_angular_acc*delta_t*delta_t;
            } else if (delta_t > t1 && delta_t <= t2) {
                res = 0.5*max_angular_acc*t1*t1+max_wc*(delta_t-t1);
            } else if (delta_t > t2 && delta_t <= t3) {
                res = angle_rad - 0.5*max_angular_acc*(delta_t-t3)*(delta_t-t3);
            } else res = angle_rad;
        }

        return res*dir;

    } else if (task_type == REVOLUTION || task_type == REVOLUTION_V2) {
        res = get_desire_pos(cur_time)/radius;
        return res*dir;
    } else {
        return res;
    }
}

float PID_Differential_Control::get_desire_vel(long cur_time) {
    float delta_t = 0.001 * (cur_time - task_ts);
    float res = 0;

    if (task_type == GO_FORWARD || task_type == GO_FORWARD_V2 || task_type == REVOLUTION || task_type == REVOLUTION_V2) {
        if(bangbang_type == 0) {
            if (delta_t <= t1) {
                res = str_vel + max_acc*delta_t;
            } else if (delta_t > t1 && delta_t <= t3) {
                res = end_vel + max_acc*(t3-delta_t);
            } else {
                res = end_vel;
            }
        }

        if (bangbang_type == 1) {
            if (delta_t <= t1) {
                res = str_vel + max_acc*delta_t;
            } else if (delta_t > t1 && delta_t <= t2) {
                res = max_vel;
            } else if (delta_t > t2 && delta_t <= t3) {
                res = end_vel + max_acc*(t3-delta_t);
            } else {
                res = end_vel;
            }
        }

        if (task_type == GO_FORWARD || task_type == GO_FORWARD_V2) {
            return res*dir;
        }

        else {
            return res;
        }
    } else {
      return res;
    }
}

float PID_Differential_Control::get_desire_pos(long cur_time) {
    float delta_t = 0.001 * (cur_time - task_ts);
    float res = 0;

    if (task_type == GO_FORWARD  || task_type == GO_FORWARD_V2 || task_type == REVOLUTION || task_type == REVOLUTION_V2) {
        if(bangbang_type == 0) {
            if (delta_t <= t1) {
                res = str_vel*delta_t + 0.5*max_acc*delta_t*delta_t;
            } else if (delta_t > t1 && delta_t <= t3) {
                res = pos_x - end_vel*(t3-delta_t) - 0.5*max_acc*(delta_t-t3)*(delta_t-t3);
            } else {
                res = pos_x;
            }
        }

        if (bangbang_type == 1){
            if (delta_t <= t1) {
                res = str_vel*delta_t + 0.5*max_acc*delta_t*delta_t;
            } else if (delta_t > t1 && delta_t <= t2) {
                res = str_vel*t1 + 0.5*max_acc*t1*t1+max_vel*(delta_t-t1);
            } else if (delta_t > t2 && delta_t <= t3) {
                res = pos_x - end_vel*(t3-delta_t) - 0.5*max_acc*(delta_t-t3)*(delta_t-t3);
            } else {
                res = pos_x;
            }
        }
      
        if (task_type == GO_FORWARD || task_type == GO_FORWARD_V2) {
            return res*dir;
        }

        else {
            return res;
        }

    } else {
      return res;
    }
}

void PID_Differential_Control::motor_run() {
    incremental_pid_motor_id(0);  
    incremental_pid_motor_id(1); 

    matrix->motorRun(left_motor_id, motor_pwm_out[0]);
    matrix->motorRun(right_motor_id, motor_pwm_out[1]);
}

void PID_Differential_Control::task_run(long cur_time) {
    if (task_type == PWM_CONTROL) {
        motor_run();
    } else if (task_type == GO_FORWARD || task_type == REVOLUTION) {
        angle_loop_pid();
        position_loop_pid();
        inverse_kinematics();
        motor_run();
    } else if (task_type == ROTATION) {
        angle_loop_pid();
        inverse_kinematics();
        motor_run();
    } else if (task_type == GO_FORWARD_V2 || task_type == REVOLUTION_V2) {
        angle_loop_pid();
        exec_vel = get_desire_vel(millis());
        posX_measure = ((left_encoder_current - left_encoder_start) + \
                        (right_encoder_current - right_encoder_start)) * 2.7150e-6;
        
        inverse_kinematics();
        motor_run();
    }
}

void PID_Differential_Control::task_reset() {
    posX_measure = 0;
    posX_err_sum = 0;
    posX_last_err = 0;
    
    angleZ_measure_degree = 0;
    angleZ_measure_rad = 0;
    angleZ_err_sum_rad = 0;
    angleZ_last_err_rad = 0;
}

void PID_Differential_Control::update() {
    delta_encoder[0] = long(left_encoder->getCount()) - last_encoder[0];
    last_encoder[0] = long(left_encoder->getCount());
    left_encoder_current = last_encoder[0];

    delta_encoder[1] = long(right_encoder->getCount()) - last_encoder[1];
    last_encoder[1] = long(right_encoder->getCount());
    right_encoder_current = last_encoder[1];

    current_update_t = millis();
    long encoder_delta_t = current_update_t - last_update_t;
    last_update_t = current_update_t;

    motor_w_measure[0] = float(delta_encoder[0]*0.1939/encoder_delta_t); // 2*PI*1000/(32400*20) = 0.0097
    motor_pwm_measure[0] = int(delta_encoder[0]*2.21618/encoder_delta_t);   // 11.428*2*PI*1000/(32400*20) = 0.11
    motor_w_measure[1] = float(delta_encoder[1]*0.1939/encoder_delta_t);  // 2*PI*1000/(32400*20) = 0.0097
    motor_pwm_measure[1] = int(delta_encoder[1]*2.21618/encoder_delta_t); // 11.428*2*PI*1000/(32400*20) = 0.11

    mpu6050->update();
    angleZ_current_degree = mpu6050->getAngleZ();
    angleZ_current_rad = angleZ_current_degree*0.017453292;
}

bool PID_Differential_Control::is_task_complete() {
    if (task_type == PWM_CONTROL) {
        return int(task_duration*1000)  < (millis() - task_ts);
    } else if (task_type == GO_FORWARD || task_type == GO_FORWARD_V2) {
        return abs(posX_measure - pos_x*dir) < 0.01;
    } else if (task_type == ROTATION) {
        return abs(angleZ_measure_degree - angle_degree*dir) < 1;
    } else if (task_type == REVOLUTION || task_type == REVOLUTION_V2 ) {
        return abs(posX_measure - pos_x) < 0.01;
    } else {
      return false;
    }
}


void PID_Differential_Control::pwmControl(int left_pwm, int right_pwm, float duration) {
    task_type = PWM_CONTROL;
    motor_pwm_desire[0] = left_pwm;
    motor_pwm_desire[1] = right_pwm;
    task_duration = duration;

    // mpu6050->update();
    task_init(millis());
    delay(1);
    while(!is_task_complete()) {
        long loop_ts = millis();
        update();
        task_run(millis());

        long loop_time = millis() - loop_ts;
        if (loop_time < control_loop_period) {
            delay(control_loop_period - loop_time);
        }
    }

    task_reset();
    stop();
}

void PID_Differential_Control::forward(float distance, float max_v) {
    task_type = GO_FORWARD;
    pos_x = distance;
    max_vel = max_v;

    task_init(millis());
    while(!is_task_complete()) {
        long loop_ts = millis();
        update();
        task_run(millis());

        long loop_time = millis() - loop_ts;
        if (loop_time < control_loop_period) {
            delay(control_loop_period - loop_time);
        }
    }

    task_reset();
    stop();
}

void PID_Differential_Control::forward(float distance, float max_v, float start_v, float end_v) {
    task_type = GO_FORWARD_V2;
    pos_x = distance;
    max_vel = max_v;
    str_vel = start_v;
    end_vel = end_v;

    task_init(millis());
    while(!is_task_complete()) {
        long loop_ts = millis();
        update();
        task_run(millis());

        long loop_time = millis() - loop_ts;
        if (loop_time < control_loop_period) {
            delay(control_loop_period - loop_time);
        }
    }

    task_reset();
    if (fabs(end_v) < 0.01) {
        stop();
    }
}

void PID_Differential_Control::rotate(float degree, float max_w) {
    task_type = ROTATION;
    angle_degree = degree;
    max_wc = max_w;
    
    task_init(millis());
    while(!is_task_complete()) {
       long loop_ts = millis();
       update();
       task_run(millis());

       long loop_time = millis() - loop_ts;
       if (loop_time < control_loop_period) {
           delay(control_loop_period - loop_time);
       }
    }

    task_reset();
    stop();
}

void PID_Differential_Control::revolve(float r, float degree, float max_v) {
    task_type = REVOLUTION;
    radius = r;
    angle_degree = degree;
    max_vel = max_v;

    task_init(millis());
    while(!is_task_complete()) {
        long loop_ts = millis();
        update();
        task_run(millis());

        long loop_time = millis() - loop_ts;
        if (loop_time < control_loop_period) {
            delay(control_loop_period - loop_time);
        }
    }

    task_reset();
    stop();
}

void PID_Differential_Control::revolve(float r, float degree, float max_v, float start_v, float end_v) {
    task_type = REVOLUTION_V2;
    radius = r;
    angle_degree = degree;
    max_vel = max_v;
    str_vel = start_v;
    end_vel = end_v;

    task_init(millis());
    while(!is_task_complete()) {
        long loop_ts = millis();
        update();
        task_run(millis());

        long loop_time = millis() - loop_ts;
        if (loop_time < control_loop_period) {
            delay(control_loop_period - loop_time);
        }
    }

    task_reset();
    if (fabs(end_v) < 0.01) {
        stop();
    }
}

void PID_Differential_Control::stop() {
    pid_err_sum[0] = 0;
    last_pid_err[0] = 0;
    last_last_pid_err[0] = 0;

    pid_err_sum[1] = 0;
    last_pid_err[1] = 0;
    last_last_pid_err[1] = 0;

    matrix->motorRun(left_motor_id, 0);
    matrix->motorRun(right_motor_id, 0);
}